﻿using Net.Common;
using Net.Component;
using System;
using System.Threading;
using System.Diagnostics;
using System.Threading.Tasks;
using System.Collections.Generic;
using System.Runtime.CompilerServices;

namespace Net.FlowField
{
    public delegate void SimulatorState(ref AgentUnit unit, int state);

    public class Simulator : Singleton<Simulator>, IDisposable
    {
        private const float LerpSpeed = 0.1f;
        private const float MoveSpeed = 0.1f;

        public Rect WorldRect;
        public Vector2 WorldOffset => WorldRect.position;
        public Vector2 GridSize;
        public int GridWidth => (int)(WorldRect.width / GridSize.x);
        public int GridHeight => (int)(WorldRect.height / GridSize.y);

        // 游戏状态
        public GridCell[] Grids;
        public AgentUnit[] Units;
        public InstanceData[] InstanceDatas;
        internal AStarNode[] NodeGrid;
        public int instanceCount = 0;
        public List<Formation> formations = new();
        private int currentFormationIndex;
        private int nextFormationId = 0;
        private Stopwatch simulationStopwatch = new();
        public long simulationDelay = 0;
        public int simulationFPS;
        private int simulationCurrentFPS;
        private DateTime simulationTime;
        private Thread simulationThread;
        public int updateFlowFieldTick;
        private bool isDisposable;
        public bool AutoStep;
        public int interval = 1;
        public SimulatorState OnEnterState;
        public SimulatorState OnUpdateState;
        public SimulatorState OnExitState;

        /// <summary>
        /// 构造函数，初始化窗体
        /// </summary>
        public void Initialize(Vector2 offset, Vector2 gridSize, Vector2 worldSize)
        {
            InitializeGrid(offset, gridSize, worldSize);
            simulationThread = new Thread(() =>
            {
                while (!isDisposable)
                {
                    Thread.Sleep(interval);
                    if (!AutoStep)
                        continue;
                    SimulateStep();
                    simulationCurrentFPS++;
                    if (DateTime.Now >= simulationTime)
                    {
                        simulationTime = DateTime.Now.AddSeconds(1d);
                        simulationFPS = simulationCurrentFPS;
                        simulationCurrentFPS = 0;
                        simulationDelay = 0;
                    }
                }
            });
            simulationThread.IsBackground = true;
            simulationThread.Start();
        }

        ~Simulator()
        {
            Grids = default;
            Units = default;
            simulationThread?.Interrupt();
            simulationThread = null;
            isDisposable = true;
        }

        /// <summary>
        /// 初始化网格系统
        /// </summary>
        private void InitializeGrid(Vector2 offset, Vector2 gridSize, Vector2 worldSize)
        {
            WorldRect = new Rect(offset, worldSize);
            GridSize = gridSize;
            Grids = new GridCell[GridWidth * GridHeight];
            Units = new AgentUnit[GridWidth * GridHeight];
            InstanceDatas = new InstanceData[GridWidth * GridHeight];
            NodeGrid = new AStarNode[GridWidth * GridHeight];
            for (int i = 0; i < Grids.Length; i++)
            {
                int x = i % GridWidth;
                int z = i / GridWidth;

                // 计算世界坐标，使(0,0)位于中心
                float worldX = x + offset.x;
                float worldZ = z + offset.y;

                Grids[i] = new GridCell
                {
                    Rect = new Rect(worldX, worldZ, GridSize.x, GridSize.y),
                    IsObstacleBool = false,
                    IsOccupiedBool = false,
                    AgentId = -1
                };
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector3 GridToWorldPosition(in Vector3 gridPos)
        {
            return new Vector3(gridPos.x * GridSize.x + WorldOffset.x, 0, gridPos.z * GridSize.y + WorldOffset.y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector3 WorldToGridPosition(in Vector3 worldPos)
        {
            return new Vector3((worldPos.x - WorldOffset.x) / GridSize.x, 0, (worldPos.z - WorldOffset.y) / GridSize.y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ref GridCell GetGridCell(in Vector3 worldPos)
        {
            var gridPos = WorldToGridPosition(worldPos);
            int index = (int)(gridPos.z * GridWidth + gridPos.x);
            return ref Grids[index];
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int WorldToGridIndex(in Vector3 worldPos)
        {
            return GetGridIndex(WorldToGridPosition(worldPos));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int GetGridIndex(in Vector3 gridPos)
        {
            return (int)(gridPos.z * GridWidth + gridPos.x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void SetObstacle(in Vector3 worldPos, bool isObstacle)
        {
            GetGridCell(worldPos).IsObstacleBool = isObstacle;
        }

        /// <summary>
        /// 在指定位置添加单位方阵
        /// </summary>
        public List<int> AddFormation(in Vector3 worldPos, Quaternion rotation, int teamType, int count, byte occupiedCount = 0, int attackPower = 50, int attackRange = 1, int pursuitRange = 200, int health = 100)
        {
            var center = WorldToGridPosition(worldPos);
            var currentFormation = GetFormation(teamType, count * count);
            // 计算方阵起始位置
            int startX = (int)Math.Max(0, center.x - count / 2);
            int startZ = (int)Math.Max(0, center.z - count / 2);
            int endX = Math.Min(GridWidth, startX + count);
            int endZ = Math.Min(GridHeight, startZ + count);
            // 在方阵范围内添加单位
            var agentUnits = new List<int>(count * count);
            for (int x = startX; x < endX; x++)
            {
                for (int z = startZ; z < endZ; z++)
                {
                    var unitId = AddUnitInternal(new Vector3(x, 0, z), rotation, teamType, currentFormation.Id, occupiedCount, attackPower, attackRange, pursuitRange, health);
                    if (unitId == -1)
                        continue;
                    agentUnits.Add(unitId);
                }
            }
            updateFlowFieldTick = 0;
            currentFormationIndex = currentFormation.Id;
            return agentUnits;
        }

        public Formation GetFormation(int teamType, int count)
        {
            Formation currentFormation = null;
            for (int i = 0; i < formations.Count; i++)
            {
                var formation = formations[i];
                if (formation.Count > 0)
                    continue;
                formation.TeamType = teamType;
                formation.Count = count;
                currentFormation = formation;
                break;
            }
            if (currentFormation == null)
            {
                currentFormation = new Formation
                {
                    Id = nextFormationId++,
                    TeamType = teamType,
                    Count = count,
                };
                formations.Add(currentFormation);
            }
            return currentFormation;
        }

        /// <summary>
        /// 在指定位置添加单个单位
        /// </summary>
        public int AddUnit(Vector3 worldPos, Quaternion rotation, int teamType, int formationId, byte occupiedCount = 0, int attackPower = 50, int attackRange = 1, int pursuitRange = 200, int health = 100)
        {
            var gridPos = WorldToGridPosition(worldPos);
            return AddUnitInternal(gridPos, rotation, teamType, formationId, occupiedCount, attackPower, attackRange, pursuitRange, health);
        }

        /// <summary>
        /// 在指定位置添加单个单位
        /// </summary>
        public int AddUnitInternal(Vector3 gridPos, Quaternion rotation, int teamType, int formationId, byte occupiedCount = 0, int attackPower = 50, int attackRange = 1, int pursuitRange = 200, int health = 100)
        {
            if (instanceCount >= Units.Length)
                return -1;
            // 检查所有将要占用的格子是否可用
            bool canPlace = true;
            if (occupiedCount == 0)
            {
                int index = GetGridIndex(gridPos);
                if (Grids[index].IsObstacleBool || Grids[index].IsOccupiedBool)
                    canPlace = false;
            }
            else
            {
                for (int dx = -occupiedCount; dx <= occupiedCount; dx++)
                {
                    for (int dz = -occupiedCount; dz <= occupiedCount; dz++)
                    {
                        int x = (int)gridPos.x + dx;
                        int z = (int)gridPos.z + dz;
                        if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                        {
                            canPlace = false;
                            break;
                        }
                        int idx = z * GridWidth + x;
                        ref var grid = ref Grids[idx];
                        if (grid.IsObstacleBool || grid.IsOccupiedBool)
                        {
                            canPlace = false;
                            break;
                        }
                    }
                    if (!canPlace) break;
                }
            }
            if (!canPlace)
                return -1;
            var worldPos = GridToWorldPosition(gridPos);
            var unitId = instanceCount;
            var unit = new AgentUnit
            {
                Id = unitId,
                Position = gridPos,
                PrePosition = gridPos,
                Active = 1,
                Type = teamType,
                Health = health,
                AttackPower = attackPower,
                AttackRange = attackRange,
                PursuitRange = pursuitRange,
                State = int.MinValue,
                Speed = MoveSpeed,
                FormationId = formationId,
                OccupiedCount = occupiedCount,
            };
            InstanceDatas[unitId] = new InstanceData()
            {
                viewMatrix = Matrix4x4.TRS(worldPos, rotation, Vector3.one),
                animScalar = Vector3.one,
            };
            OnEnterState?.Invoke(ref unit, 0);
            Units[unitId] = unit;
            // 标记所有占用的格子
            if (occupiedCount == 0)
            {
                int index = GetGridIndex(gridPos);
                Grids[index].IsOccupiedBool = true;
                Grids[index].AgentId = unitId;
            }
            else
            {
                for (int dx = -occupiedCount; dx <= occupiedCount; dx++)
                {
                    for (int dz = -occupiedCount; dz <= occupiedCount; dz++)
                    {
                        int x = (int)gridPos.x + dx;
                        int z = (int)gridPos.z + dz;
                        int idx = z * GridWidth + x;
                        Grids[idx].IsOccupiedBool = true;
                        Grids[idx].AgentId = unitId;
                    }
                }
            }
            instanceCount++;
            return unitId;
        }

        /// <summary>
        /// 更新所有编队的流场
        /// </summary>
        private void UpdateFlowFields()
        {
            if (Environment.TickCount < updateFlowFieldTick)
                return;
            updateFlowFieldTick = Environment.TickCount + 500;
            var formation = formations[currentFormationIndex++ % formations.Count];
            if (formation.Count <= 0)
                return;
            var selfCenterV2 = formation.BoundingRect.center;
            var selfCenter = new Vector3(selfCenterV2.x, 0, selfCenterV2.y);
            var distance = float.MaxValue;
            Formation formationTarget = null;
            Vector3 target = default;
            foreach (var formation1 in formations)
            {
                if (formation1.Count <= 0 || formation1 == formation || formation.TeamType == formation1.TeamType)
                    continue;
                var targetCenterV2 = formation1.BoundingRect.center;
                var targetCenter = new Vector3(targetCenterV2.x, 0, targetCenterV2.y);
                var dis = Vector3.Distance(selfCenter, targetCenter);
                if (dis < distance)
                {
                    distance = dis;
                    formationTarget = formation1;
                    target = targetCenter;
                }
            }
            if (formationTarget == null)
                formation.Target = formation.TeamType == 0 ? new Vector3(GridWidth - 1, 0, GridHeight / 2) : new Vector3(0, 0, GridHeight / 2);
            else
                formation.Target = target;
            formation.CalculatePath(this, formation.Target);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private bool IsPositionValid(Vector3 position)
        {
            return position.x >= 0 && position.x < GridWidth && position.z >= 0 && position.z < GridHeight;
        }

        public void ResetSimulation()
        {
            instanceCount = 0;
            formations.Clear();
            nextFormationId = 0;
        }

        public void SimulateStep()
        {
            simulationStopwatch.Restart();
            ResetFormations();
            MoveAndAttackUnits();
            RemoveDeadUnits();
            UpdateFlowFields();
            simulationStopwatch.Stop();
            if (simulationStopwatch.ElapsedMilliseconds > simulationDelay)
                simulationDelay = simulationStopwatch.ElapsedMilliseconds;
        }

        private void ResetFormations()
        {
            for (int i = 0; i < formations.Count; i++)
            {
                var formation = formations[i];
                if (formation.Count <= 0)
                    continue;
                formation.BoundingRect = new RectInt(formation.MinX, formation.MinZ, formation.MaxX - formation.MinX + 1, formation.MaxZ - formation.MinZ + 1);
                formation.MinX = int.MaxValue;
                formation.MinZ = int.MaxValue;
                formation.MaxX = int.MinValue;
                formation.MaxZ = int.MinValue;
            }
        }

        public Vector3 CameraPosition;
        public Vector4[] FrustumPlanes = new Vector4[6];
        public float[] LODDis = new float[] { 50f, 100f, 1000f };

        private bool IsVisible(in Vector3 position)
        {
            for (int i = 0; i < 6; i++)
            {
                var plane = FrustumPlanes[i];
                if (Vector3.Dot(plane, position) + plane.w < 0)
                {
                    return false;
                }
            }
            return true;
        }

        private int SelectLOD(float dis)
        {
            for (int i = 0; i < LODDis.Length; i++)
            {
                if (dis < LODDis[i])
                    return i;
            }
            return -1;
        }

        private void MoveAndAttackUnits()
        {
            var result = Parallel.For(0, instanceCount, i =>
            {
                ref var unit = ref Units[i];
                if (unit.Health <= 0)
                    return;
                var formation = formations[unit.FormationId];
                formation.InterlockedBounding(unit.Position);

                // lod
                var worldPos = GridToWorldPosition(unit.PrePosition);
                var visible = IsVisible(worldPos);
                if (visible)
                {
                    float dis = Vector3.Distance(worldPos, CameraPosition);
                    int lod = SelectLOD(dis);
                    unit.LodId = lod;
                }

                if (unit.State == 1)
                {
                    var direction = unit.Position - unit.PrePosition;
                    var magnitude = direction.magnitude;
                    if (magnitude <= unit.Speed)
                    {
                        unit.PrePosition = unit.Position;
                        unit.State = -1;
                    }
                    else
                    {
                        unit.PrePosition += direction / magnitude * unit.Speed;
                    }
                    LerpPositionAndRotation(ref unit, GridToWorldPosition(unit.PrePosition), direction);
                    OnUpdateState?.Invoke(ref unit, 1);
                    return;
                }
                if (unit.State == 2)
                {
                    if (Environment.TickCount >= unit.Cooldown)
                        unit.State = -2;
                    return;
                }
                Vector3 nextPathPos;
                ref var closestEnemy = ref FindClosestEnemy(unit);
                if (closestEnemy != AgentUnit.Default)
                {
                    nextPathPos = closestEnemy.Position;
                    var dis = Vector3.Distance(unit.Position, nextPathPos);
                    if (dis <= unit.AttackRange + (closestEnemy.AttackRange / 2))
                    {
                        if (unit.State != 2)
                            OnEnterState?.Invoke(ref unit, 2);
                        unit.State = 2;
                        OnUpdateState?.Invoke(ref unit, 2);
                        closestEnemy.Health = Math.Max(0, closestEnemy.Health - unit.AttackPower);
                        InstanceDatas[closestEnemy.Id].emissionColor = new Vector4(50, 1, 1, 1);
                        unit.Cooldown = Environment.TickCount + 500;
                        return;
                    }
                }
                else
                {
                    var distance = float.MaxValue;
                    for (int x = 0; x < formation.Path.Count; x++)
                    {
                        var dis = Vector3.Distance(unit.Position, formation.Path[x]);
                        if (dis < distance)
                        {
                            distance = dis;
                            unit.PathIndex = x + 1;
                        }
                    }
                    nextPathPos = formation.GetNextPathPosition(unit.PathIndex);
                }
                int dx = Math.Sign(nextPathPos.x - unit.Position.x);
                int dz = Math.Sign(nextPathPos.z - unit.Position.z);
                if (TryMoveUnit(ref unit, new Vector3(dx, 0, dz), out var position))
                {
                    MoveUnitToPosition(ref unit, position);
                    if (unit.State != 1 && unit.State != -1)
                        OnEnterState?.Invoke(ref unit, 1);
                    unit.State = 1;
                    OnUpdateState?.Invoke(ref unit, 1);
                }
                else
                {
                    if (unit.State != 0)
                        OnEnterState?.Invoke(ref unit, 0);
                    unit.State = 0;
                    OnUpdateState?.Invoke(ref unit, 0);
                }
            });
            while (!result.IsCompleted) ;
        }

        public void LerpPositionAndRotation(ref AgentUnit unit, Vector3 worldPos, Vector3 direction)
        {
            ref var instanceData = ref InstanceDatas[unit.Id];

            // 获取当前矩阵的位置和旋转
            var currentPos = instanceData.viewMatrix.GetPosition();
            var currentRot = instanceData.viewMatrix.GetRotation();

            // 计算插值后的位置和旋转
            var lerpedPos = Vector3.Lerp(currentPos, worldPos, LerpSpeed);
            var targetRot = Quaternion.LookRotation(direction, Vector3.up);
            var lerpedRot = Quaternion.Slerp(currentRot, targetRot, LerpSpeed);

            // 直接修改矩阵（避免创建新Matrix4x4）
            ref var m = ref instanceData.viewMatrix;

            // ---- 手动设置旋转部分 ----
            float xx = lerpedRot.x * lerpedRot.x;
            float yy = lerpedRot.y * lerpedRot.y;
            float zz = lerpedRot.z * lerpedRot.z;
            float xy = lerpedRot.x * lerpedRot.y;
            float xz = lerpedRot.x * lerpedRot.z;
            float yz = lerpedRot.y * lerpedRot.z;
            float wx = lerpedRot.w * lerpedRot.x;
            float wy = lerpedRot.w * lerpedRot.y;
            float wz = lerpedRot.w * lerpedRot.z;

            m.m00 = 1.0f - 2.0f * (yy + zz);
            m.m10 = 2.0f * (xy + wz);
            m.m20 = 2.0f * (xz - wy);

            m.m01 = 2.0f * (xy - wz);
            m.m11 = 1.0f - 2.0f * (xx + zz);
            m.m21 = 2.0f * (yz + wx);

            m.m02 = 2.0f * (xz + wy);
            m.m12 = 2.0f * (yz - wx);
            m.m22 = 1.0f - 2.0f * (xx + yy);

            // ---- 设置位置部分 ----
            m.m03 = lerpedPos.x;
            m.m13 = lerpedPos.y;
            m.m23 = lerpedPos.z;

            // ---- 固定值部分 ----
            m.m30 = 0;
            m.m31 = 0;
            m.m32 = 0;
            m.m33 = 1;
        }

        private bool TryMoveUnit(ref AgentUnit unit, Vector3 direction, out Vector3 position)
        {
            /*
             *      向量图示（XZ平面坐标系）
             *
             * Z
             * ↑   (-1,1)        (0,1)       (1,1)
             * 1+     ↖           ↑           ↗
             *  |       \          |          /
             *  |        \         |         /
             *  |         \        |        /
             * 0+---- ← (-1,0) ——(0,0)—— (1,0) → X
             *  |         /        |        \
             *  |        /         |         \
             *  |       /          |          \
             *        ↙            ↓          ↘ 
             * -1+  (-1,-1)       (0,-1)      (1,-1)
             * 
             */
            if (TryMoveDirection(ref unit, direction, out position))
                return true;
            if (direction.x == 1 && direction.z == 0) // -> 右边移动
            {
                // ↗ 右上角移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, 1), out position))
                    return true;

                // ↘ 右下角移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, -1), out position))
                    return true;
            }
            if (direction.x == -1 && direction.z == 0) // -> 左边移动
            {
                // ↖ 左上角移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, 1), out position))
                    return true;

                // ↙ 左下角移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, -1), out position))
                    return true;
            }
            if (direction.x == 0 && direction.z == 1) // -> 向上移动
            {
                // ↖ 左上角移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, 1), out position))
                    return true;

                // ↗ 右上角移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, 1), out position))
                    return true;
            }
            if (direction.x == 0 && direction.z == -1) // -> 向下移动
            {
                // ↙ 左下角移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, -1), out position))
                    return true;

                // ↘ 右下角移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, -1), out position))
                    return true;
            }
            if (direction.x == 1 && direction.z == 1) // -> 右上角移动
            {
                // ↑ 向上移动
                if (TryMoveDirection(ref unit, new Vector3(0, 0, 1), out position))
                    return true;

                // → 向右移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, 0), out position))
                    return true;
            }
            if (direction.x == 1 && direction.z == -1) // -> 右下角移动
            {
                // → 向右移动
                if (TryMoveDirection(ref unit, new Vector3(1, 0, 0), out position))
                    return true;

                // ↓ 向下移动
                if (TryMoveDirection(ref unit, new Vector3(0, 0, -1), out position))
                    return true;
            }
            if (direction.x == -1 && direction.z == 1) // -> 左上角移动
            {
                // ↑ 向上移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, 0), out position))
                    return true;

                // ← 向左移动
                if (TryMoveDirection(ref unit, new Vector3(0, 0, 1), out position))
                    return true;
            }
            if (direction.x == -1 && direction.z == -1) // -> 左下角移动
            {
                // ↑ 向上移动
                if (TryMoveDirection(ref unit, new Vector3(-1, 0, 0), out position))
                    return true;

                // ← 向左移动
                if (TryMoveDirection(ref unit, new Vector3(0, 0, -1), out position))
                    return true;
            }
            position = default;
            return false;
        }

        private void MoveUnitToPosition(ref AgentUnit unit, Vector3 position)
        {
            // 1. 检查新位置所有格子是否可占用
            bool canOccupy = true;
            if (unit.OccupiedCount == 0)
            {
                int idx = GetGridIndex(position);
                if (Grids[idx].IsObstacleBool || Grids[idx].IsOccupiedBool)
                    canOccupy = false;
            }
            else
            {
                for (int dx = -unit.OccupiedCount; dx <= unit.OccupiedCount; dx++)
                {
                    for (int dz = -unit.OccupiedCount; dz <= unit.OccupiedCount; dz++)
                    {
                        int x = (int)position.x + dx;
                        int z = (int)position.z + dz;
                        if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                        {
                            canOccupy = false;
                            break;
                        }
                        int idx = z * GridWidth + x;
                        ref var grid = ref Grids[idx];
                        if ((grid.IsObstacleBool || grid.IsOccupiedBool) && grid.AgentId != unit.Id)
                        {
                            canOccupy = false;
                            break;
                        }
                    }
                    if (!canOccupy) break;
                }
            }
            if (!canOccupy)
                return;

            // 2. 释放旧格子
            if (unit.OccupiedCount == 0)
            {
                int idx = GetGridIndex(unit.Position);
                Grids[idx].IsOccupied = 0;
                Grids[idx].AgentId = -1;
                Grids[idx].IsOccupiedBool = false;
            }
            else
            {
                for (int dx = -unit.OccupiedCount; dx <= unit.OccupiedCount; dx++)
                {
                    for (int dz = -unit.OccupiedCount; dz <= unit.OccupiedCount; dz++)
                    {
                        int x = (int)unit.Position.x + dx;
                        int z = (int)unit.Position.z + dz;
                        if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                            continue;
                        int idx = z * GridWidth + x;
                        Grids[idx].IsOccupied = 0;
                        Grids[idx].AgentId = -1;
                        Grids[idx].IsOccupiedBool = false;
                    }
                }
            }

            // 3. 占用新格子
            if (unit.OccupiedCount == 0)
            {
                int idx = (int)(position.z * GridWidth + position.x);
                //Interlocked.CompareExchange(ref Grids[idx].IsOccupied, 1, 0);
                Grids[idx].AgentId = unit.Id;
                Grids[idx].IsOccupiedBool = true;
            }
            else
            {
                for (int dx = -unit.OccupiedCount; dx <= unit.OccupiedCount; dx++)
                {
                    for (int dz = -unit.OccupiedCount; dz <= unit.OccupiedCount; dz++)
                    {
                        int x = (int)position.x + dx;
                        int z = (int)position.z + dz;
                        if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                            continue;
                        int idx = z * GridWidth + x;
                        //Interlocked.CompareExchange(ref Grids[idx].IsOccupied, 1, 0);
                        Grids[idx].AgentId = unit.Id;
                        Grids[idx].IsOccupiedBool = true;
                    }
                }
            }

            // 4. 更新单位位置
            unit.Position = position;
        }

        private bool TryMoveDirection(ref AgentUnit unit, Vector3 direction, out Vector3 position)
        {
            var newPosition = unit.Position + direction;
            bool canMove = true;
            if (unit.OccupiedCount == 0)
            {
                if (IsPositionValid(newPosition))
                {
                    int index = GetGridIndex(newPosition);
                    ref var currentGrid = ref Grids[index];
                    if (!currentGrid.IsObstacleBool && !currentGrid.IsOccupiedBool)
                    {
                        position = newPosition;
                        return true;
                    }
                }
                position = default;
                return false;
            }
            else
            {
                for (int dx = -unit.OccupiedCount; dx <= unit.OccupiedCount; dx++)
                {
                    for (int dz = -unit.OccupiedCount; dz <= unit.OccupiedCount; dz++)
                    {
                        int x = (int)newPosition.x + dx;
                        int z = (int)newPosition.z + dz;
                        if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                        {
                            canMove = false;
                            break;
                        }
                        int idx = z * GridWidth + x;
                        ref var grid = ref Grids[idx];
                        if ((grid.IsObstacleBool || grid.IsOccupiedBool) && grid.AgentId != unit.Id)
                        {
                            canMove = false;
                            break;
                        }
                    }
                    if (!canMove) break;
                }
                if (canMove)
                {
                    position = newPosition;
                    return true;
                }
                position = default;
                return false;
            }
        }

        private ref AgentUnit FindClosestEnemy(in AgentUnit attacker)
        {
            int closestDistance = int.MaxValue;
            ref AgentUnit closestEnemy = ref AgentUnit.Default;
            // 获取当前位置
            int centerX = (int)attacker.Position.x;
            int centerZ = (int)attacker.Position.z;
            // 从近到远搜索敌人
            for (int r = 1; r <= attacker.PursuitRange; r++)
            {
                int allDirectionsBlockedByFriendlies = 0;
                // 搜索当前半径范围内的所有格子
                for (int dx = -r; dx <= r; dx++)
                {
                    for (int dz = -r; dz <= r; dz++)
                    {
                        // 跳过中心点和超出当前半径的点
                        if (dx == 0 && dz == 0)
                            continue;
                        int checkX = centerX + dx;
                        int checkZ = centerZ + dz;
                        // 边界检查
                        if (checkX < 0 || checkX >= GridWidth || checkZ < 0 || checkZ >= GridHeight)
                            continue;
                        // 安全计算索引
                        int index = checkZ * GridWidth + checkX;
                        if (index < 0 || index >= Grids.Length)
                            continue;
                        ref var grid = ref Grids[index];
                        if (grid.IsObstacleBool || !grid.IsOccupiedBool || grid.AgentId == attacker.Id)
                            continue;
                        int agentId = grid.AgentId; //并行时需要先获取, 否则直线下面的是并行修改成-1就会出问题
                        if (agentId == -1)
                            continue;
                        ref AgentUnit unit = ref Units[agentId];
                        if (unit.Type == attacker.Type)
                        {
                            allDirectionsBlockedByFriendlies++;
                            continue;
                        }
                        if (unit.Active == 0 || unit.Health <= 0)
                            continue;
                        // 计算距离并记录最近的敌人
                        int distance = Math.Abs(dx) + Math.Abs(dz);
                        if (distance < closestDistance)
                        {
                            closestDistance = distance;
                            closestEnemy = ref unit;
                        }
                    }
                }
                // 如果8个方向都被友军阻挡，停止搜索
                if (allDirectionsBlockedByFriendlies >= 8)
                    return ref closestEnemy;
                // 如果已经找到最近的敌人，提前返回
                if (closestEnemy != AgentUnit.Default)
                    return ref closestEnemy;
            }
            return ref closestEnemy;
        }

        private void RemoveDeadUnits()
        {
            for (int i = 0; i < instanceCount; i++)
            {
                var unit = Units[i];
                if (unit.Active == 0 || unit.Health > 0)
                    continue;
                if (unit.State != 3)
                    OnEnterState?.Invoke(ref unit, 3);
                unit.State = 3;
                OnUpdateState?.Invoke(ref unit, 3);
                unit.Active = 0;
                formations[unit.FormationId].Count--;
                // 释放所有占用的格子
                if (unit.OccupiedCount == 0)
                {
                    int idx = (int)(unit.Position.z * GridWidth + unit.Position.x);
                    Grids[idx].AgentId = -1;
                    Grids[idx].IsOccupiedBool = false;
                }
                else
                {
                    for (int dx = -unit.OccupiedCount; dx <= unit.OccupiedCount; dx++)
                    {
                        for (int dz = -unit.OccupiedCount; dz <= unit.OccupiedCount; dz++)
                        {
                            int x = (int)unit.Position.x + dx;
                            int z = (int)unit.Position.z + dz;
                            if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                                continue;
                            int idx = z * GridWidth + x;
                            Grids[idx].AgentId = -1;
                            Grids[idx].IsOccupiedBool = false;
                        }
                    }
                }
                int lastIndex = --instanceCount;
                if (i >= lastIndex)
                    continue;
                var lastUnit = Units[lastIndex];
                if (lastUnit.Active == 0)
                {
                    instanceCount--;
                    continue;
                }
                // 更新交换单位的占用格子
                if (lastUnit.OccupiedCount == 0)
                {
                    int idx = GetGridIndex(lastUnit.Position);
                    Grids[idx].AgentId = i;
                    Grids[idx].IsOccupiedBool = true;
                }
                else
                {
                    for (int dx = -lastUnit.OccupiedCount; dx <= lastUnit.OccupiedCount; dx++)
                    {
                        for (int dz = -lastUnit.OccupiedCount; dz <= lastUnit.OccupiedCount; dz++)
                        {
                            int x = (int)lastUnit.Position.x + dx;
                            int z = (int)lastUnit.Position.z + dz;
                            if (x < 0 || x >= GridWidth || z < 0 || z >= GridHeight)
                                continue;
                            int idx = z * GridWidth + x;
                            Grids[idx].AgentId = i;
                            Grids[idx].IsOccupiedBool = true;
                        }
                    }
                }
                lastUnit.Id = i;
                unit.Id = lastIndex;
                Units[i] = lastUnit;
                Units[lastIndex] = unit;
                InstanceDatas[i] = InstanceDatas[lastIndex];
            }
        }

        public void Dispose()
        {
            isDisposable = true;
        }
    }
}